﻿using SoftFloat;
using System;
using System.Globalization;
using System.Runtime.CompilerServices;

namespace BepuUtilities
{
    public struct Quaternion
    {
        public sfloat X, Y, Z, W;

        public Quaternion(sfloat x, sfloat y, sfloat z, sfloat w)
        {
            X = x;
            Y = y;
            Z = z;
            W = w;
        }

        public static Quaternion Identity
        {
            get
            {
                return new Quaternion(0f, 0f, 0f, 1f);
            }
        }

        public static Quaternion Concatenate(Quaternion value1, Quaternion value2)
        {
            sfloat x = value2.X;
            sfloat y = value2.Y;
            sfloat z = value2.Z;
            sfloat w = value2.W;
            sfloat x2 = value1.X;
            sfloat y2 = value1.Y;
            sfloat z2 = value1.Z;
            sfloat w2 = value1.W;
            sfloat num = y * z2 - z * y2;
            sfloat num2 = z * x2 - x * z2;
            sfloat num3 = x * y2 - y * x2;
            sfloat num4 = x * x2 + y * y2 + z * z2;
            Quaternion result;
            result.X = x * w2 + x2 * w + num;
            result.Y = y * w2 + y2 * w + num2;
            result.Z = z * w2 + z2 * w + num3;
            result.W = w * w2 - num4;
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Quaternion Conjugate(Quaternion value)
        {
            return Multiply(value, new Vector4(-1f, -1f, -1f, 1f));
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        internal static Quaternion Multiply(Quaternion value1, Vector4 value2)
        {
            return new Quaternion(value1.X * value2.X, value1.Y * value2.Y, value1.Z * value2.Z, value1.W * value2.W);
        }

        public static Quaternion CreateFromAxisAngle(Vector3 axis, sfloat angle)
        {
            sfloat x = angle * 0.5f;
            sfloat num = libm.Sin(x);
            sfloat w = libm.Cos(x);
            Quaternion result;
            result.X = axis.X * num;
            result.Y = axis.Y * num;
            result.Z = axis.Z * num;
            result.W = w;
            return result;
        }

        public static Quaternion CreateFromYawPitchRoll(sfloat yaw, sfloat pitch, sfloat roll)
        {
            sfloat x = roll * 0.5f;
            sfloat num = libm.Sin(x);
            sfloat num2 = libm.Cos(x);
            sfloat x2 = pitch * 0.5f;
            sfloat num3 = libm.Sin(x2);
            sfloat num4 = libm.Cos(x2);
            sfloat x3 = yaw * 0.5f;
            sfloat num5 = libm.Sin(x3);
            sfloat num6 = libm.Cos(x3);
            Quaternion result;
            result.X = num6 * num3 * num2 + num5 * num4 * num;
            result.Y = num5 * num4 * num2 - num6 * num3 * num;
            result.Z = num6 * num4 * num - num5 * num3 * num2;
            result.W = num6 * num4 * num2 + num5 * num3 * num;
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public readonly sfloat Length()
        {
            sfloat x = LengthSquared();
            return libm.Sqrt(x);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public readonly sfloat LengthSquared()
        {
            return Dot(this, this);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static float Dot(Quaternion quaternion1, Quaternion quaternion2)
        {
            return quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y + quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static bool operator ==(Quaternion value1, Quaternion value2)
        {
            return value1.X == value2.X && value1.Y == value2.Y && value1.Z == value2.Z && value1.W == value2.W;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static bool operator !=(Quaternion value1, Quaternion value2)
        {
            return !(value1 == value2);
        }

        public override string ToString()
        {
            return $"({X.ToString("G")}, {Y.ToString("G")}, {Z.ToString("G")}, {W.ToString("G")})";
        }
    }
}